#include <stdio.h>
#include <string.h>
#include <stdint.h>
#include <vector>

#include "opencv/cv.h"
#include "opencv/highgui.h"
#include "picornt.h"

using namespace std;

#ifdef WIN32
#ifdef _DEBUG
#pragma comment(lib, "opencv_core310d.lib")
#pragma comment(lib, "opencv_imgproc310d.lib")
#pragma comment(lib, "opencv_flann310d.lib")
#pragma comment(lib, "opencv_highgui310d.lib")
#pragma comment(lib, "opencv_calib3d310d.lib")
#pragma comment(lib, "opencv_videoio310d.lib")
#pragma comment(lib, "opencv_imgcodecs310d.lib")
#pragma comment(lib, "opencv_objdetect310d.lib")
#else
#pragma comment(lib, "opencv_core310.lib")
#pragma comment(lib, "opencv_imgproc310.lib")
#pragma comment(lib, "opencv_flann310.lib")
#pragma comment(lib, "opencv_highgui310.lib")
#pragma comment(lib, "opencv_features2d310.lib")
#pragma comment(lib, "opencv_video310.lib")
#pragma comment(lib, "opencv_calib3d310.lib")
#pragma comment(lib, "opencv_videoio310.lib")
#pragma comment(lib, "opencv_imgcodecs310.lib")
#pragma comment(lib, "opencv_viz310.lib")
#pragma comment(lib, "opencv_objdetect310.lib")
#endif
#endif


#define STR_IMAGE_LIST          "F:/dataset/car/test.txt"
#define STR_CASCADE             "car0325"

int main()
{
    FILE *fp_list       = fopen(STR_IMAGE_LIST, "r");
    FILE *fp_cascade    = fopen(STR_CASCADE, "rb");
    if (!fp_list || !fp_cascade) {
        printf("ERROR:: \n");

        return -1;
    }

    void* cascade = 0;

    fseek(fp_cascade, 0L, SEEK_END);
    int32_t size = ftell(fp_cascade);
    fseek(fp_cascade, 0L, SEEK_SET);

    cascade = malloc(size);

    if (!cascade || size != fread(cascade, 1, size, fp_cascade))
        return 1;

    fclose(fp_cascade);

    int32_t i, j;
    int32_t iimg = 0;
    char filename[1024];
    while (fscanf(fp_list, "%s", filename) != NULL) {
        IplImage *pimshow = cvLoadImage(filename, CV_LOAD_IMAGE_COLOR);

        if (!pimshow) {
            printf("ERROR:: invalid file name!\n");
            return -1;
        }

        CvMat *pimsrc = cvCreateMat(pimshow->height, pimshow->width, CV_8UC1);
        cvCvtColor(pimshow, pimsrc, CV_BGR2GRAY);

        iimg++;
        int64_t t;
        // perform detection with the pico library
        t = cvGetTickCount();

#define MAXNDETECTIONS 2048
        float rcsq[4 * MAXNDETECTIONS];
        int ndetections;
        int nd;
        int nrows, ncols, ldim;

        /* config */
        float scalefactor = 1.1f;
        float stridefactor = 0.1;
        int32_t minsize = 256;
        int32_t maxsize = 2048;

        uint8_t* pixels;

        if (1)
        {
            static IplImage* gray = 0;
            static IplImage* pyr[5] = { 0, 0, 0, 0, 0 };
            if(!pyr[0])
            {
                gray = cvCreateImage(cvSize(pimsrc->width, pimsrc->height), 8, 1);

                pyr[0] = gray;
                pyr[1] = cvCreateImage(cvSize(pimsrc->width / 2, pimsrc->height / 2), 8, 1);
                pyr[2] = cvCreateImage(cvSize(pimsrc->width / 4, pimsrc->height / 4), 8, 1);
                pyr[3] = cvCreateImage(cvSize(pimsrc->width / 8, pimsrc->height / 8), 8, 1);
                pyr[4] = cvCreateImage(cvSize(pimsrc->width / 16, pimsrc->height / 16), 8, 1);
            }

            cvCopy(pimsrc, gray);

            pyr[0] = gray;

            pixels = (uint8_t*)pyr[0]->imageData;
            nrows = pyr[0]->height;
            ncols = pyr[0]->width;
            ldim = pyr[0]->widthStep;

            int64_t tick0 = cv::getTickCount();
            ndetections = find_objects(rcsq, MAXNDETECTIONS, cascade, 0, pixels, nrows, ncols, ldim, scalefactor, stridefactor, MAX(16, minsize), MIN(128, maxsize));
            int64_t tick1 = cv::getTickCount();
            const double tick_freq = cv::getTickFrequency();
            printf("time consume (ms): %f\n",
                (tick1 - tick0)*1000.f / tick_freq);

            for (i = 1; i < 5; ++i)
            {
                cvResize(pyr[i - 1], pyr[i], CV_INTER_LINEAR);

                pixels = (uint8_t*)pyr[i]->imageData;
                nrows = pyr[i]->height;
                ncols = pyr[i]->width;
                ldim = pyr[i]->widthStep;

                nd = find_objects(&rcsq[4 * ndetections], MAXNDETECTIONS - ndetections, cascade, 0, pixels, nrows, ncols, ldim, scalefactor, stridefactor, MAX(64, minsize >> i), MIN(128, maxsize >> i));

                for (j = ndetections; j < ndetections + nd; ++j)
                {
                    rcsq[4 * j + 0] = (1 << i)*rcsq[4 * j + 0];
                    rcsq[4 * j + 1] = (1 << i)*rcsq[4 * j + 1];
                    rcsq[4 * j + 2] = (1 << i)*rcsq[4 * j + 2];
                }

                ndetections = ndetections + nd;
            }
        }
        else
        {
            //
            pixels = (uint8_t*)pimsrc->data.ptr;
            nrows = pimsrc->height;
            ncols = pimsrc->width;
            ldim = pimsrc->step;

            //
            ndetections = find_objects(rcsq, MAXNDETECTIONS, cascade, 0, pixels, nrows, ncols, ldim, scalefactor, stridefactor, minsize, MIN(nrows, ncols));
        }

        ndetections = cluster_detections(rcsq, ndetections);

        t = cvGetTickCount() - t;

        for (i = 0; i < ndetections; ++i)
            if (rcsq[4 * i + 3] >= 3.f) // check the confidence threshold
                cvCircle(pimshow, cvPoint(rcsq[4 * i + 1], rcsq[4 * i + 0]), rcsq[4 * i + 2] / 2, CV_RGB(255, 0, 0), 4, 8, 0); // we draw circles here since height-to-width ratio of the detected face regions is 1.0f

        cvShowImage("src", pimshow);
        cvWaitKey();

        cvReleaseImage(&pimshow);
        cvReleaseMat(&pimsrc);
    }

    return 0;
}